#ifndef PERCEPTION_SENSORLIDAR_LIDAR_MOVING_OBJECT_DETECTION_H
#define PERCEPTION_SENSORLIDAR_LIDAR_MOVING_OBJECT_DETECTION_H

#include <Eigen/Dense>

#include <ros/ros.h>

#include <pcl/point_cloud.h>

#include <opencv2/core/core.hpp>

#include "common/object_types.h"
#include "utils/rigid.h"
#include "utils/timed_queue.h"

namespace avos {
namespace perception {

using ::common::transform::Rigid3;
using ::common::transform::StampedStorage;
using ::common::transform::TimeCache;
using ::common::transform::TimedQueue;

/// The occupancy grid map class.
/**
 * Maintain a accumulative grid map 
 * based on a durable time that an objects accumulated grid dense
 */

template <typename T>
class GridMapOccupancy {
 public:
  GridMapOccupancy()
      : resolution_(0.0),
        max_x_(0.0),
        max_y_(0.0),
        origin_x_(0.0),
        origin_y_(0.0) {}
  GridMapOccupancy(float range_x, float range_y, float resolution)
      : resolution_(resolution) {
    max_x_ = range_x / resolution_;
    max_y_ = range_y / resolution_;
    // FIXME: custom origin x y
    origin_x_ = floor(max_x_ / 2.0);
    origin_y_ = max_y_ * 1.0 / 3.0;
    Reset();
  }
  GridMapOccupancy(const GridMapOccupancy& rhs) = default;
  GridMapOccupancy& operator=(const GridMapOccupancy& rhs) = default;
  /// Get current min x coordinate based in Lidar.
  float GetXMin() { return -origin_x_ * resolution_; }
  /// Get current max x coordinate based in Lidar.
  float GetXMax() { return (max_x_ - origin_x_) * resolution_; }
  /// Get current min y coordinate based in Lidar.
  float GetYMin() { return -origin_y_ * resolution_; }
  /// Get current max y coordinate based in Lidar.
  float GetYMax() { return (max_y_ - origin_y_) * resolution_; }

  /// Reset two-dimensional variable to zero.
  void Reset() {
    data_ = DummyVector<DummyVector<T>>(max_x_, DummyVector<T>(max_y_, 0));
  }
  /// Judge if x and y coordinate in ROI range.
  inline bool InRange(const int& x, const int& y) {
    return (x >= 0 && x < max_x_ && y >= 0 && y < max_y_);
  }
  /// Get data index in gridmap from rx and ry coordinate that Lidar based.
  /**
   * \param[in] rx The x coordinate on Lidar system.
   * \param[in] ry The y coordinate on Lidar system.
   * \param[in] x The x coordinate on gridmap.
   * \param[in] y The y coordinate on gridmap.
   */
  void GetIndex(const float& rx, const float& ry, int& x, int& y) {
    x = rx / resolution_ + origin_x_;
    y = ry / resolution_ + origin_y_;
  }
  /// Set data value in gridmap based on x and y index.
  /**
   * \param[in] x The x coordinate on gridmap.
   * \param[in] y The y coordinate on gridmap.
   * \param[in] value Gridmap value in x and y index.
   */
  void Set(const int& x, const int& y, const T value) { data_[x][y] = value; }
  /// Get data value in gridmap based on x and y index.
  T Value(const int& x, const int& y) { return data_[x][y]; }

  T* MutableValue(const int& x, const int& y) { return &data_[x][y]; }

 private:
  /// gridmap size factor.
  float resolution_;
  /// max gridmap x value.
  int max_x_;
  /// max gridmap y value.
  int max_y_;
  /// a origin x value for translate coordinate from Lidar to gridmap.
  int origin_x_;
  /// a origin y value for translate coordinate from Lidar to gridmap.
  int origin_y_;
  /// Two-dimensional array that stored gridmap data.
  DummyVector<DummyVector<T>> data_;
};

/// Cells stored coordinate make up with current frame pose.
struct CellsWithPose2 {
  DummyVector<Eigen::Vector3f> cells;
  Rigid3<float> pose;

  CellsWithPose2(const DummyVector<Eigen::Vector3f>& in_cells,
                 const Rigid3<float>& in_pose)
      : cells(in_cells), pose(in_pose) {}
  CellsWithPose2() = default;
  CellsWithPose2(const CellsWithPose2& rhs) = default;
  CellsWithPose2& operator=(const CellsWithPose2& rhs) = default;
};

/// The moving object detection class.
/**
 * Using grid map to calculate objects occupying dense in a durable time 
 * to detect objects which are moving or static.
 */

class MovingObjectDetection {
 public:
  MovingObjectDetection(double stamped_buffer_length, double latency);
  /// Lidar_objects filter to correct misclassified objects.
  /**
   * \param[in] data_time The ros time value.
   * \param[in] lidar_objects Lidar objects in one points frame.
   * \param[in] current_pose Current pose get from localization.
   */
  void Filter(ros::Time data_time, DummyVector<LidarObject>& lidar_objects,
              Rigid3<float> current_pose);
  /// Visualize gridmap points for debug.
  void Visualize(pcl::PointCloud<pcl::PointXYZ>& cells_cloud);

 private:
  const double time_latency_;
  GridMapOccupancy<uint8_t> grid_map_;
  TimedQueue<CellsWithPose2> cells_with_pose_list_;

  // DEBUG value
  DummyVector<Eigen::Vector3f> accumulate_local_cells_;
};
}  // namespace perception
}  // namespace avos
#endif  // PERCEPTION_SENSORLIDAR_LIDAR_MOVING_OBJECT_DETECTION_H